home *** CD-ROM | disk | FTP | other *** search
/ Aminet 7 / Aminet 7 - August 1995.iso / Aminet / misc / sci / RARS_Amiga_3.lha / RARS / heath2.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-05-27  |  8.3 KB  |  284 lines

  1. // Heath2.cpp - "driver" function for RARS - 
  2. // Patrick Tierney, May. 1995
  3. // g2kafka@cdf.toronto.edu
  4.  
  5. #include <string.h>
  6. #include <stdlib.h>
  7. #include <math.h>
  8. #include "car.h"
  9.  
  10. // These parameters may be adjusted to get better performance:
  11. const double h3_csc = 5.400577;   
  12. const double h3_sg = 1.760864; 
  13. const double h3_sd = 0.899998;
  14. const double h3_ca = 0.089994;
  15. const double h3_mb = 0.299592;
  16. const double h3_ea = 0.052816;
  17. const double h3_sl = 6.503468;
  18. const double h3_sc = 895.618408;
  19. const double h3_st = 0.055179;
  20. const double h3_DF = 88.586960;
  21. const double h3_CDF = 66.340027;
  22. const double h3_pow = 11.865677;
  23. const double h3_er = 90.589142;
  24. const double h3_tf = 7.906990;
  25. const double h3_bs = 2.079712;
  26. const double h3_bf = 2.604871;
  27. const double h3_as = -0.073984;
  28. const double h3_vs = 379.745483;
  29. const double h3_oa = 0.855200;
  30. const double h3_ss1 = 3.491272;
  31. const double h3_ss2 = 4.742371;
  32. const double h3_ss3 = 3.133423;
  33. const double h3_ss4 = 0.835815;
  34. const double h3_ss5 = 0.510529;
  35. const double h3_ss6 = 0.419733;
  36. const double h3_ct1 = 3.709598;
  37. const double h3_ct2 = 0.321442;
  38. const double h3_ct3 = 0.453116;
  39. const double h3_cv1 = 4.712219;
  40. const double h3_cv2 = 1.813950;
  41. const double h3_cv3 = 3.240390;
  42. const double h3_cv4 = 2.050635;
  43. const double h3_cw1 = 0.488956;
  44. const double h3_cw2 = 0.634894;
  45. const double h3_cw3 = 1.833496;
  46. const double h3_cw4 = 11.118134;
  47. const double h3_cw5 = 1.052383;
  48. const double h3_cw6 = 0.283782;
  49. const double h3_cw7 = 4.727951;
  50. const double h3_cw8 = 0.471585;
  51.  
  52.  
  53. const int h3_PT = 61;  // time to stay in passing maneuver, counts
  54.  
  55. inline double h3_ABS(double arg) {return arg < 0.0 ? -arg: arg;}
  56.  
  57. double h3_corSpd(double radius)
  58. {
  59.    if(radius < 0.0)         // change sign of negative radius
  60.       radius = -radius;
  61.    else if(radius == 0.0)   // This is just insurance, this funtion doesn't
  62.       return(300.0);        // make sense when the radius is zero.
  63.    return h3_csc * sqrt(radius+h3_er);
  64.  
  65. }
  66.  
  67. double h3_whlSpd(double goal, double present)
  68. {
  69.    double ws;
  70.  
  71.    if(present > goal + h3_tf* h3_sl)  // if way too fast,
  72.       ws = present - h3_bs*h3_sl;      // slow down.
  73.    else if(present > goal + 2 * h3_sl)  // if too fast,
  74.       ws = present - h3_sl;      // slow down.
  75.    else if(present < goal - 2 * h3_sl)  // if too slow,
  76.       ws = present + h3_bf*h3_sl;             // accelerate.
  77.    else                           // if quite close,
  78.       ws = (goal + present) / 2;      // approach desired speed gently.
  79.  
  80.    return ws;
  81. }
  82.  
  83. con_vec Heath2(situation& s)
  84. {
  85.    const char name[] = "Heath2";      // This is the robot driver's name!
  86.    static int init_flag = 1;          // cleared by first call
  87.    double speed;                      // target speed for cornering, ft/sec
  88.    double speed_next;                 // target speed for next corner
  89.    con_vec result;                    // This is what is returned.
  90.    double width;                      // track width, feet
  91.    double dist;
  92.    double add_rad;
  93.    double the_rad;
  94.    double the_rad2;
  95.    double head;
  96.    double proj_out;
  97.    double ggg;
  98.    double lll;
  99.    int    str_type;
  100.    int    curve_type;
  101.    double alpha, vc;           // components of result
  102.    static double alpha_inc = 0.0;  // alpha increment during passing maneuver
  103.    static int counting = 0;    // will be set and counting down when passing
  104.  
  105.    if(init_flag)  {            // first time through, only copy name:
  106. //pjt_fptr=fopen("zzz.out","wt");
  107.       my_name_is(name);
  108.       init_flag = 0;
  109.       result.alpha = result.vc = 0;
  110.       return result;
  111.    }
  112.  
  113. if(s.starting){
  114.     result.alpha=h3_as;
  115.     result.vc =s.v+h3_vs;
  116.     return result;
  117.     }
  118.   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  119.       return result;
  120.  
  121.    // Set alpha based on a servo-mechanism approach, trying to stay
  122.    // in the middle of the track, i.e., s.to_left equal to .5 * width:
  123.    width = s.to_lft + s.to_rgt;  // find width of track
  124.    curve_type=0;
  125.    if(s.cur_rad==0.0){
  126.         if(s.nex_rad>0.0)
  127.             dist=h3_oa;
  128.         else
  129.             dist=1.0-h3_oa;;
  130.         if(s.cur_len < h3_ss1*width){
  131.             str_type=1;
  132.             if(s.cur_len > h3_ss2*width){
  133.                 ggg=s.to_end-width;
  134.                 lll=s.cur_len-width;
  135.                 }
  136.             else{
  137.                 ggg=0.0;
  138.                 lll=1.0;
  139.                 }
  140.             }
  141.         else{
  142.             str_type=0;
  143.             ggg=s.to_end-h3_ss3*width;
  144.             lll=s.cur_len-h3_ss3*width;
  145.             }
  146.         if((!str_type)&&(ggg>0.0))
  147.             dist=(ggg*(s.to_lft/width - dist) + lll*dist)/lll;
  148.         else
  149.             if((str_type)&&(s.to_end>h3_ss4*width))
  150.                 dist=((ggg*(s.to_lft/width - dist) + lll*dist)/lll+0.5)/2.0;
  151.             else
  152.                 dist=h3_ss5;
  153.         alpha = h3_ss6*h3_sg * (s.to_lft - dist * width) / width;
  154.         }
  155.    else{
  156.         // check here for a straight-curve
  157.         curve_type=1;
  158.         the_rad=h3_ABS(s.cur_rad/s.cur_len);
  159.         if((s.cur_rad*s.nex_rad<=0.0)&&(width > h3_ct1 * the_rad*(sqrt(2.0/(1.0+cos(s.cur_len)))-1))){
  160.             // curve is fairly straight
  161.             dist=(the_rad/width+0.5)*sqrt((1.0+cos(s.cur_len))/2.0)/
  162.                     cos(s.to_end-s.cur_len/2.0)-the_rad/width;
  163.             curve_type=2;
  164.             }
  165.         else
  166.             dist=pow(h3_ABS(2.0*s.to_end/s.cur_len-1.0),h3_pow)*(h3_ss5-h3_ct2)+h3_ct2;
  167.         if(s.cur_rad > 0.0){
  168.             alpha = h3_ct3*h3_sg * (s.to_lft - dist * width) / width;
  169.             alpha+=h3_ca;
  170.             }
  171.         else{
  172.             alpha = h3_ct3*h3_sg * (s.to_lft - (1.0-dist) * width) / width;
  173.             alpha-=h3_ca;
  174.             }
  175.         }
  176.    alpha -= h3_sd * s.vn / s.v;  // This is damping, to prevent oscillation
  177.  
  178.    // calculate target speeds for current corner and the next:
  179.    if(curve_type==2)
  180.         speed = h3_corSpd(h3_cv1*s.cur_rad);
  181.    else
  182.         if((curve_type==1)&&(s.cur_rad*s.nex_rad<=0.0)&&(s.cur_len<1.0))
  183.             speed=h3_corSpd(h3_cv2*s.cur_rad);
  184.         else
  185.             speed = h3_corSpd(s.cur_rad);      // speed is based on radius
  186.    the_rad2=h3_ABS(s.nex_rad/s.nex_len);
  187.    if((s.nex_rad*s.after_rad<=0.0)&&(width > h3_cv3 * the_rad2*(sqrt(2.0/(1.0+cos(s.nex_len)))-1)))
  188.         speed_next = h3_corSpd(h3_cv4*s.nex_rad);
  189.    else
  190.         speed_next = h3_corSpd(s.nex_rad); // of center line of track.
  191.  
  192.    // now set the tire speed, vc:
  193.    if(s.cur_rad == 0.0){                  // If we are on a straightaway,
  194.       if((str_type)&&(h3_ABS(s.nex_rad)>h3_cw1))
  195.         speed_next=h3_cw2*speed_next;
  196.       if(s.v < sqrt(h3_DF*s.to_end+speed_next*speed_next))
  197.          vc = s.v + h3_cw3*h3_sc / s.v; // keep accellerating near full power
  198.       else{                    // otherwise,
  199.         if(s.to_end < 2.0*h3_mb * s.cur_len){
  200.             if(s.nex_rad > 0)
  201.                 alpha+=h3_ca/h3_cw4;
  202.             else
  203.                 alpha-=h3_ca/h3_cw4;
  204.             }
  205.          if (s.v>h3_cw5*speed_next)
  206.             vc=h3_cw6*s.v;
  207.          else
  208.             vc = h3_whlSpd(speed_next, s.v);    // brake for next corner
  209.         }
  210.       }
  211.    else{          // If we're in the curve, maintain speed.
  212.       if(s.cur_rad >0.0)
  213.         add_rad=s.to_lft;
  214.       else
  215.         add_rad=s.to_rgt;
  216.       head=(2.0*asin(s.vn/s.v)+alpha)/4.0;
  217.       if(s.to_end<=1.57)
  218.         proj_out=(the_rad+add_rad)/(cos(s.to_end)+tan(head)*sin(s.to_end));
  219.       if((s.to_end > 1.57)||(proj_out>(the_rad+width))||(proj_out<the_rad))
  220.           // if we are far from the next corner, stay at "speed".
  221.          vc = h3_whlSpd(speed, s.v);
  222.       else{  // but when we near the next corner, adjust to "speed_next"
  223.          if(s.cur_rad>0)
  224.             alpha -=h3_ca/h3_cw4;
  225.          else
  226.             alpha +=h3_ca/h3_cw4;
  227.          if (s.nex_rad !=0){
  228.             if(s.v < sqrt(h3_CDF*s.to_end/s.cur_len*h3_ABS(s.cur_rad)+speed_next*speed_next))
  229.                 vc=h3_whlSpd(speed_next, s.v);
  230.             else
  231.                 vc=h3_cw6*s.v;
  232.             }
  233.          else{
  234.             if(s.nex_len<h3_cw7*width)
  235.                 speed_next=h3_cw8*speed_next;
  236.             vc = h3_whlSpd(speed_next, s.v);
  237.             }
  238.         }
  239.     }
  240.    // The passing maneuver:
  241.    if(s.dead_ahead & !counting)  {  // When first encountering the car ahead:
  242.      counting = h3_PT;         // setup the timer,
  243.      if(s.cur_rad !=0.0){
  244.         vc=0.25*s.v;
  245.         counting=0;
  246.         }
  247.      else if((s.to_lft>s.to_rgt)&&(s.nearby[0].rel_xdot>=0))
  248.         alpha_inc=h3_st;
  249.      else if((s.to_lft<s.to_rgt)&&(s.nearby[0].rel_xdot<=0))
  250.         alpha_inc=-h3_st;
  251.      else if(random(10) <2){
  252.         vc=0.25*s.v;
  253.         counting=0;
  254.         }
  255.      else if(s.cur_rad==0.0){
  256.       if(random(29999) < 15000)        // choose a right or left maneuver:
  257.          alpha_inc = h3_st;
  258.       else
  259.          alpha_inc = -h3_st;
  260.      }
  261.    else if(s.cur_rad >0.0){
  262.       if(random(29999) < 5000)        // choose a right or left maneuver:
  263.          alpha_inc = h3_st;
  264.       else
  265.          alpha_inc = -h3_st;
  266.      }
  267.   else{
  268.       if(random(29999) < 25000)        // choose a right or left maneuver:
  269.          alpha_inc = h3_st;
  270.       else
  271.          alpha_inc = -h3_st;
  272.      }
  273.    }
  274.    if(counting)  {                // If we are still in the passing maneuver,
  275.       alpha += alpha_inc;              // change alpha
  276.       --counting;                      // count down to zero
  277.    }
  278.  
  279.    result.vc = vc;   result.alpha = alpha;
  280.    return result;
  281. }
  282.  
  283.  
  284.